/**
  ******************************************************************************
  *Copyright(C),Tongji University
  * @file           : HMC5883L_stm32f4xx_hal.c
  * @brief          : contain the magnetometer functions 
  * Author			: ultralyj
  * Date			: 2020.10.27
  ******************************************************************************
  */

#include "HMC5883L_stm32f4xx_hal.h"
#include "main.h"

extern I2C_HandleTypeDef hi2c1;
extern TIM_HandleTypeDef htim2;
uint16_t HMC5883LYaw = 0,GetYawBusy=0,SettingYaw=0;
/**
  * @brief HMC5883L, the magnetometer Initialization Function
  * @retval None
  */
void HAL_HMC5883L_Init(void)
{
	uint8_t v = 0;
	v = 0x70;
	HAL_I2C_Mem_Write(&hi2c1, HMC5883L_ADDRESS << 1, HMC5883L_CONFIGURATION_REGISTERA,
					  I2C_MEMADD_SIZE_8BIT, &v, 1, 0xfff);
	v = 0x20;
	HAL_I2C_Mem_Write(&hi2c1, HMC5883L_ADDRESS << 1, HMC5883L_CONFIGURATION_REGISTERB,
					  I2C_MEMADD_SIZE_8BIT, &v, 1, 0xfff);
	/* Measurement mode configration */
	v = MEASUREMENT_CONTINUOUS;
	HAL_I2C_Mem_Write(&hi2c1, HMC5883L_ADDRESS << 1, HMC5883L_MODE_REGISTER,
					  I2C_MEMADD_SIZE_8BIT, &v, 1, 0xfff);
	HAL_TIM_Base_Start_IT(&htim2);
}

/**
  * @brief Get Raw magdata from HMC5883L
  * @retval HMC5883L_DateType, the structure contains x,y,z-axis data
  */
HMC5883L_DateType HAL_HMC5883L_GetRawData(void)
{
	HMC5883L_DateType ans;
	uint8_t rawdata[6];
	HAL_I2C_Mem_Read(&hi2c1, HMC5883L_ADDRESS << 1, HMC5883L_DATAX_MSB,
					 I2C_MEMADD_SIZE_8BIT, &rawdata[0], 1, 0xfff);
	HAL_I2C_Mem_Read(&hi2c1, HMC5883L_ADDRESS << 1, HMC5883L_DATAX_LSB,
					 I2C_MEMADD_SIZE_8BIT, &rawdata[1], 1, 0xfff);
	HAL_I2C_Mem_Read(&hi2c1, HMC5883L_ADDRESS << 1, HMC5883L_DATAY_MSB,
					 I2C_MEMADD_SIZE_8BIT, &rawdata[2], 1, 0xfff);
	HAL_I2C_Mem_Read(&hi2c1, HMC5883L_ADDRESS << 1, HMC5883L_DATAY_LSB,
					 I2C_MEMADD_SIZE_8BIT, &rawdata[3], 1, 0xfff);
	HAL_I2C_Mem_Read(&hi2c1, HMC5883L_ADDRESS << 1, HMC5883L_DATAZ_MSB,
					 I2C_MEMADD_SIZE_8BIT, &rawdata[4], 1, 0xfff);
	HAL_I2C_Mem_Read(&hi2c1, HMC5883L_ADDRESS << 1, HMC5883L_DATAZ_LSB,
					 I2C_MEMADD_SIZE_8BIT, &rawdata[5], 1, 0xfff);

	/* Add MSB and LSB */
	ans.gx = (rawdata[0] << 8) | rawdata[1];
	ans.gy = (rawdata[2] << 8) | rawdata[3];
	ans.gz = (rawdata[4] << 8) | rawdata[5];
	/* Transform from complement code */
	if (ans.gx > 0x7fff)
		ans.gx = -(0xffff - ans.gx);
	if (ans.gy > 0x7fff)
		ans.gy = -(0xffff - ans.gy);
	if (ans.gz > 0x7fff)
		ans.gz = -(0xffff - ans.gz);
	return ans;
}

/**
  * @brief Get Yaw angle from HMC5883L
  * @retval HMC5883L_DateType, the structure contains x,y,z-axis data
  */
uint16_t HAL_HMC5883L_GetYaw(void)
{
	int32_t sum = 0;
	static HMC5883L_DateType HMC5883L_FIFO[HMC5883L_FIFO_NUM + 1];
	HMC5883L_DateType dat = HAL_HMC5883L_GetRawData();
	/* Simple Blurring Procession */
	for (uint8_t i = 1; i < HMC5883L_FIFO_NUM; i++)
		HMC5883L_FIFO[i - 1] = HMC5883L_FIFO[i];
	HMC5883L_FIFO[HMC5883L_FIFO_NUM - 1] = dat;
	sum = 0;
	for (uint8_t i = 0; i < HMC5883L_FIFO_NUM; i++)
		sum += HMC5883L_FIFO[i].gx;
	HMC5883L_FIFO[HMC5883L_FIFO_NUM].gx = sum / HMC5883L_FIFO_NUM;
	sum = 0;
	for (uint8_t i = 0; i < HMC5883L_FIFO_NUM; i++)
		sum += HMC5883L_FIFO[i].gy;
	HMC5883L_FIFO[HMC5883L_FIFO_NUM].gy = sum / HMC5883L_FIFO_NUM;
	sum = 0;
	for (uint8_t i = 0; i < HMC5883L_FIFO_NUM; i++)
		sum += HMC5883L_FIFO[i].gz;
	HMC5883L_FIFO[HMC5883L_FIFO_NUM].gz = sum / HMC5883L_FIFO_NUM;
	/* Calculate the Yaw angle*/
	double x = HMC5883L_FIFO[HMC5883L_FIFO_NUM].gx;
	double y = HMC5883L_FIFO[HMC5883L_FIFO_NUM].gy;
	uint16_t Angle = (uint16_t)(atan2(y, x) * ((double)180 / 3.14159265) + 180);
	return Angle;
}

uint16_t HAL_HMC5883L_GetYawSafe(void)
{
	while(GetYawBusy);
	return HMC5883LYaw;
}
